/**
 * @file 2025E.c
 * @brief 飞机巡航路径规划系统
 * @author Jim
 * @date 2025
 *
 * 功能：在串口屏上实现飞机巡航路径显示
 * 地图：9×7网格（A1-A9, B1-B7），720×560像素，每格80×80像素
 * 策略：蛇形扫描法，遇到禁飞区向下绕行
 */

#include "2025E.h"
#include "oled.h"
#include "Waypoint.h"
#include <math.h>
#include "usart.h"

// 外部函数声明
extern void RecordNoFlyZone(uint8_t row, uint8_t col);

// OLED显示管理
static uint8_t oled_line = 0; // 当前显示行
#define MAX_OLED_LINES 8      // OLED最大行数

// 显示模式控制
static uint8_t fast_mode = 0; // 0=正常模式(有延时), 1=快速模式(无延时)

// 地图参数定义
#define MAP_WIDTH 720  // 地图宽度(像素)
#define MAP_HEIGHT 560 // 地图高度(像素)
#define GRID_SIZE 80   // 每个格子大小(像素)
#define GRID_ROWS 7    // 网格行数(B1-B7)
#define GRID_COLS 9    // 网格列数(A1-A9)
#define TOTAL_GRIDS 63 // 总格子数

// 格子状态定义
#define GRID_NORMAL 0 // 正常格子
#define GRID_NO_FLY 1 // 禁飞区
#define GRID_START 2  // 起点

/**
 * @brief 格子坐标结构体
 */
typedef struct
{
    uint16_t center_x; // 格子中心点X坐标
    uint16_t center_y; // 格子中心点Y坐标
    uint8_t row;       // 行号 (0=B1, 1=B2, ..., 6=B7)
    uint8_t col;       // 列号 (0=A, 1=B, ..., 8=I)
    uint8_t status;    // 格子状态
    char name[4];      // 格子名称 (如"A1", "B7")
} GridCell_t;

/**
 * @brief 路径段结构体
 */
typedef struct
{
    uint16_t from_x, from_y; // 起点坐标
    uint16_t to_x, to_y;     // 终点坐标
    uint16_t color;          // 线条颜色
} PathSegment_t;

// 全局变量
static GridCell_t grid_map[GRID_ROWS][GRID_COLS]; // 网格地图
static PathSegment_t cruise_path[200];            // 巡航路径
static uint16_t path_count = 0;                   // 路径段数量
static uint8_t nofly_counter = 0;                 // 禁飞区计数器
static uint8_t map_initialized = 0;               // 地图初始化标志

/**
 * @brief OLED显示调试信息（替代printf）
 * @param msg 要显示的消息
 */
void OLED_DebugMsg(char *msg)
{
    if (oled_line >= MAX_OLED_LINES)
    {
        OLED_Clear(); // 清屏重新开始
        oled_line = 0;
    }

    OLED_ShowString(0, oled_line, msg, 12, 0);
    oled_line++;

    // 根据模式决定是否延时
    if (!fast_mode)
        HAL_Delay(50);
}

/**
 * @brief OLED显示数字信息
 * @param msg 消息前缀
 * @param num 要显示的数字
 */
void OLED_DebugNum(char *msg, uint32_t num)
{
    if (oled_line >= MAX_OLED_LINES)
    {
        OLED_Clear();
        oled_line = 0;
    }

    OLED_ShowString(0, oled_line, msg, 12, 0);
    OLED_ShowNum(60, oled_line, num, 5, 12, 0);
    oled_line++;

    if (!fast_mode)
        HAL_Delay(50);
}

/**
 * @brief OLED显示坐标信息
 * @param name 格子名称
 * @param x X坐标
 * @param y Y坐标
 */
void OLED_DebugCoord(char *name, uint16_t x, uint16_t y)
{
    if (oled_line >= MAX_OLED_LINES)
    {
        OLED_Clear();
        oled_line = 0;
    }

    OLED_ShowString(0, oled_line, name, 12, 0);
    OLED_ShowNum(30, oled_line, x, 3, 12, 0);
    OLED_ShowString(60, oled_line, ",", 12, 0);
    OLED_ShowNum(70, oled_line, y, 3, 12, 0);
    oled_line++;

    if (!fast_mode)
        HAL_Delay(30);
}

/**
 * @brief 清空OLED显示
 */
void OLED_ClearDebug(void)
{
    OLED_Clear();
    oled_line = 0;
}

/**
 * @brief 设置显示模式
 * @param mode 0=正常模式(有延时), 1=快速模式(无延时)
 */
void SetDisplayMode(uint8_t mode)
{
    fast_mode = mode;
    if (fast_mode)
    {
        OLED_DebugMsg("Fast Mode ON");
    }
    else
    {
        OLED_DebugMsg("Normal Mode");
    }
}

/**
 * @brief 初始化网格地图坐标
 * @note 按照你的思路：把64个方格的坐标封装好
 */
void InitGridMap(void)
{
   

    for (uint8_t row = 0; row < GRID_ROWS; row++)
    {
        for (uint8_t col = 0; col < GRID_COLS; col++)
        {
            // 计算格子中心坐标
            grid_map[row][col].center_x = col * GRID_SIZE + GRID_SIZE / 2;
            // Y坐标反向：B1在底部，B7在顶部
            grid_map[row][col].center_y = (GRID_ROWS - 1 - row) * GRID_SIZE + GRID_SIZE / 2;
            grid_map[row][col].row = row;
            grid_map[row][col].col = col;
            grid_map[row][col].status = GRID_NORMAL;

            // 生成格子名称 (A1, A2, ..., I7)
            sprintf(grid_map[row][col].name, "%c%d", 'A' + col, row + 1);
        }
    }

    // 设置起点（右下角）
    grid_map[GRID_ROWS - 1][GRID_COLS - 1].status = GRID_START;

    // OLED_DebugMsg("Grid Init OK");
    // OLED_DebugNum("Grids:", TOTAL_GRIDS);
}

/**
 * @brief 设置单个禁飞区格子
 * @param row 行号 (0-6)
 * @param col 列号 (0-8)
 */
void SetNoFlyZone(uint8_t row, uint8_t col)
{
    if (row >= GRID_ROWS || col >= GRID_COLS)
    {
      //  OLED_DebugMsg("NoFlsy: Out Bound");
        return;
    }

    // 设置单个格子为禁飞区
    grid_map[row][col].status = GRID_NO_FLY;
   // OLED_DebugMsg("Set NoFly:");
    //OLED_DebugMsg(grid_map[row][col].name);
}

/**
 * @brief 检查格子是否可访问
 * @param row 行号
 * @param col 列号
 * @return 1:可访问, 0:不可访问
 */
uint8_t IsGridAccessible(uint8_t row, uint8_t col)
{
    if (row >= GRID_ROWS || col >= GRID_COLS)
        return 0;

    return (grid_map[row][col].status != GRID_NO_FLY);
}

/**
 * @brief 添加路径段
 * @param from_x 起点X
 * @param from_y 起点Y
 * @param to_x 终点X
 * @param to_y 终点Y
 * @param color 线条颜色
 */
void AddPathSegment(uint16_t from_x, uint16_t from_y, uint16_t to_x, uint16_t to_y, uint16_t color)
{
    if (path_count < 200)
    {
        cruise_path[path_count].from_x = from_x;
        cruise_path[path_count].from_y = from_y;
        cruise_path[path_count].to_x = to_x;
        cruise_path[path_count].to_y = to_y;
        cruise_path[path_count].color = color;
        path_count++;
    }
}

/**
 * @brief 蛇形扫描巡航路径生成（核心算法）
 * @note 实现你的思路：路径规划变成选方格，遇到禁飞区就绕行
 * @return 生成的路径段数量
 */
// 简化的行分析结构体（用于画线函数）
typedef struct {
    uint8_t has_obstacles;        // 是否有障碍物
    uint8_t first_obstacle_col;   // 第一个障碍物列号
    uint8_t last_obstacle_col;    // 最后一个障碍物列号
    uint8_t obstacle_count;       // 障碍物数量
} SimpleRowAnalysis_t;

// 简化的行障碍物分析函数（内联在画线函数中）
SimpleRowAnalysis_t AnalyzeRowForDisplay(uint8_t row)
{
    SimpleRowAnalysis_t analysis = {0};
    analysis.first_obstacle_col = GRID_COLS;  // 初始化为无效值
    analysis.last_obstacle_col = 0;

    for (uint8_t col = 0; col < GRID_COLS; col++)
    {
        if (!IsGridAccessible(row, col))
        {
            analysis.has_obstacles = 1;
            analysis.obstacle_count++;

            if (analysis.first_obstacle_col == GRID_COLS)
            {
                analysis.first_obstacle_col = col;  // 记录第一个障碍物
            }
            analysis.last_obstacle_col = col;  // 更新最后一个障碍物
        }
    }

    return analysis;
}
uint16_t GenerateSnakeCruisePath(void)
{
    path_count = 0;  // 重置路径计数
    uint16_t current_x, current_y;
    uint16_t next_x, next_y;
    uint8_t current_row, current_col;

    // 从起点开始（右下角A9,B1）
    current_row = 0;              // B1行（底部）
    current_col = GRID_COLS - 1;  // A9列（最右）
    current_x = grid_map[current_row][current_col].center_x;
    current_y = grid_map[current_row][current_col].center_y;

    OLED_ClearDebug();  // 清屏开始显示路径生成过程
    OLED_DebugMsg("Gen Path Start:");
    OLED_DebugCoord(grid_map[current_row][current_col].name, current_x, current_y);

    // 蛇形扫描：从B1(row=0)往上到B7(row=6)，每行交替方向
    for (uint8_t row = 0; row < GRID_ROWS; row++)
    {
        if (row % 2 == 0)  // 偶数行(B1,B3,B5,B7)：从右到左
        {
            for (int8_t col = GRID_COLS - 1; col >= 0; col--)
            {
                if (!IsGridAccessible(row, col))
                {
                    // 遇到禁飞区，垂直绕行（跳过整个禁飞区）
                    OLED_DebugMsg("NoFly Found:");
                    OLED_DebugMsg(grid_map[row][col].name);

                    // 找到禁飞区的结束位置（从右到左扫描）
                    int8_t bypass_col = col;
                    while (bypass_col >= 0 && !IsGridAccessible(row, bypass_col))
                    {
                        bypass_col--;  // 继续向左找到禁飞区结束
                    }

                    // 修正：如果禁飞区延伸到左边缘，需要区分处理
                    if (bypass_col < 0)
                    {
                        // 特殊情况2：偶数行的左边界禁飞区，需要向上绕行
                        if (row > 0 && row < GRID_ROWS - 1)  // B3, B5行（排除B1和B7）
                        {
                            // 左边界禁飞区：两段式绕行（向上绕行，不回到原行）
                            OLED_DebugMsg("Left Edge 2-Segment Bypass");

                            // 1. 垂直向上到绕行行（从A4B3到A4B4，而不是从禁飞区A3B3）
                            uint16_t start_x = grid_map[row][col + 1].center_x;      // A4B3中心X（col+1是A4）
                            uint16_t start_y = grid_map[row][col + 1].center_y;      // A4B3中心Y
                            uint16_t end_x = grid_map[row + 1][col + 1].center_x;    // A4B4中心X（col+1是A4）
                            uint16_t end_y = grid_map[row + 1][col + 1].center_y;    // A4B4中心Y
                            AddPathSegment(start_x, start_y, end_x, end_y, 0xF800);

                            // 2. 水平移动到A1B4（从A4B4到A1B4，在绕行行）
                            uint16_t next_start_x = end_x;                            // A4B4的X
                            uint16_t next_start_y = end_y;                            // A4B4的Y
                            uint16_t next_end_x = grid_map[row + 1][0].center_x;      // A1B4的X
                            uint16_t next_end_y = end_y;                              // 保持在B4行
                            AddPathSegment(next_start_x, next_start_y, next_end_x, next_end_y, 0xF800);

                            // 更新当前位置到A1B4
                            current_x = next_end_x;
                            current_y = next_end_y;
                            OLED_DebugMsg("Up Bypass to A1B4");

                            // 结束当前行，因为已经到达A1，且不回到原行
                            col = -1;
                            continue;
                        }
                        else if (row == GRID_ROWS - 1)  // 只有B7行直接结束
                        {
                            // B7行：直接结束当前行
                            OLED_DebugMsg("Edge Row NoFly End");
                            col = -1;  // 结束当前行
                            continue;
                        }
                        // B1行（row=0）不在这里处理，继续执行后面的专门逻辑
                    }

                    if (row == 0)  // B1在最底部，向上绕行
                    {
                        // 修正：B1行左边界禁飞区使用两段式绕行
                        if (bypass_col < 0)  // 左边界禁飞区（如A1A2A3）
                        {
                            // B1行左边界禁飞区：两段式绕行（向上绕行，不回到原行）
                            OLED_DebugMsg("B1 Left Edge 2-Segment Bypass");

                            // 1. 垂直向上到绕行行（从B1A4到B2A4，而不是从禁飞区B1A3）
                            uint16_t start_x = grid_map[row][col + 1].center_x;      // B1A4中心X（col+1是A4）
                            uint16_t start_y = grid_map[row][col + 1].center_y;      // B1A4中心Y
                            uint16_t end_x = grid_map[row + 1][col + 1].center_x;    // B2A4中心X（col+1是A4）
                            uint16_t end_y = grid_map[row + 1][col + 1].center_y;    // B2A4中心Y
                            AddPathSegment(start_x, start_y, end_x, end_y, 0xF800);

                            // 2. 水平移动到B2A1（从B2A4到B2A1，在绕行行，不回到原行）
                            uint16_t next_start_x = end_x;                            // B2A4的X
                            uint16_t next_start_y = end_y;                            // B2A4的Y
                            uint16_t next_end_x = grid_map[row + 1][0].center_x;      // B2A1的X
                            uint16_t next_end_y = end_y;                              // 保持在B2行
                            AddPathSegment(next_start_x, next_start_y, next_end_x, next_end_y, 0xF800);

                            // 更新当前位置到B2A1
                            current_x = next_end_x;
                            current_y = next_end_y;
                            OLED_DebugMsg("B1 Up Bypass to B2A1");

                            // 结束当前行，因为已经到达A1，且不回到原行
                            col = -1;
                        }
                        else if (row < GRID_ROWS - 1 && IsGridAccessible(row + 1, bypass_col))
                        {
                            // 普通情况：三段式向上绕行
                            // 1. 垂直向上到绕行行（在当前位置）
                            uint16_t detour_x = current_x;
                            uint16_t detour_y = grid_map[row + 1][col].center_y;
                            AddPathSegment(current_x, current_y, detour_x, detour_y, 0xF800);

                            // 2. 水平移动跳过禁飞区（在绕行行）
                            uint16_t safe_x = grid_map[row + 1][bypass_col].center_x;
                            uint16_t safe_y = detour_y;
                            AddPathSegment(detour_x, detour_y, safe_x, safe_y, 0xF800);

                            // 3. 垂直回到原行（在禁飞区后面）
                            next_x = safe_x;
                            next_y = grid_map[row][bypass_col].center_y;
                            AddPathSegment(safe_x, safe_y, next_x, next_y, 0xF800);

                            current_x = next_x;
                            current_y = next_y;
                            OLED_DebugMsg("Up Bypass Done");

                            // 跳过整个禁飞区
                            col = bypass_col;
                        }
                    }
                    else  // B2-B7都向下绕行
                    {
                        if (row > 0 && IsGridAccessible(row - 1, bypass_col))
                        {
                            // 正确的绕行逻辑：
                            // 1. 垂直向下到绕行行（在当前位置）
                            uint16_t detour_x = current_x;
                            uint16_t detour_y = grid_map[row - 1][col].center_y;
                            AddPathSegment(current_x, current_y, detour_x, detour_y, 0xF800);

                            // 2. 水平移动跳过禁飞区（在绕行行）
                            uint16_t safe_x = grid_map[row - 1][bypass_col].center_x;
                            uint16_t safe_y = detour_y;
                            AddPathSegment(detour_x, detour_y, safe_x, safe_y, 0xF800);

                            // 3. 垂直回到原行（在禁飞区后面）
                            next_x = safe_x;
                            next_y = grid_map[row][bypass_col].center_y;
                            AddPathSegment(safe_x, safe_y, next_x, next_y, 0xF800);

                            current_x = next_x;
                            current_y = next_y;
                            OLED_DebugMsg("Down Bypass Done");

                            // 跳过整个禁飞区
                            col = bypass_col;
                        }
                    }
                    continue;  // 跳过禁飞区格子
                }

                // 正常格子，添加路径
                next_x = grid_map[row][col].center_x;
                next_y = grid_map[row][col].center_y;

                if (current_x != next_x || current_y != next_y)  // 避免重复添加同一点
                {
                    AddPathSegment(current_x, current_y, next_x, next_y, 0x07E0); // 绿色正常路径
                    OLED_DebugMsg("Normal:");
                    OLED_DebugCoord(grid_map[row][col].name, next_x, next_y);
                }

                current_x = next_x;
                current_y = next_y;
            }
        }
        else  // 奇数行(B2,B4,B6)：从左到右
        {
            for (uint8_t col = 0; col < GRID_COLS; col++)
            {
                if (!IsGridAccessible(row, col))
                {
                    // 遇到禁飞区，垂直绕行（跳过整个禁飞区）
                    OLED_DebugMsg("NoFly Found:");
                    OLED_DebugMsg(grid_map[row][col].name);

                    // 找到禁飞区的结束位置（从左到右扫描）
                    uint8_t bypass_col = col;
                    while (bypass_col < GRID_COLS && !IsGridAccessible(row, bypass_col))
                    {
                        bypass_col++;  // 继续向右找到禁飞区结束
                    }

                    // 修正：如果禁飞区延伸到边缘，需要区分处理
                    if (bypass_col >= GRID_COLS)
                    {
                        // 特殊情况5：B2-B7行的右边界禁飞区，需要向上绕行
                        if (row > 0 && row < GRID_ROWS - 1)  // B2-B6行
                        {
                            // 右边界禁飞区：两段式绕行（向上绕行，不回到原行）
                            OLED_DebugMsg("Right Edge 2-Segment Bypass");

                            // 1. 垂直向上到绕行行（从A6B2到A6B3，而不是从禁飞区A7B2）
                            uint16_t start_x = grid_map[row][col - 1].center_x;      // A6B2中心X（col-1是A6）
                            uint16_t start_y = grid_map[row][col - 1].center_y;      // A6B2中心Y
                            uint16_t end_x = grid_map[row + 1][col - 1].center_x;    // A6B3中心X（col-1是A6）
                            uint16_t end_y = grid_map[row + 1][col - 1].center_y;    // A6B3中心Y
                            AddPathSegment(start_x, start_y, end_x, end_y, 0xF800);

                            // 2. 水平移动到A9B3（从A6B3到A9B3，在绕行行）
                            uint16_t next_start_x = end_x;                                    // A6B3的X
                            uint16_t next_start_y = end_y;                                    // A6B3的Y
                            uint16_t next_end_x = grid_map[row + 1][GRID_COLS - 1].center_x;  // A9B3的X
                            uint16_t next_end_y = end_y;                                      // 保持在B3行
                            AddPathSegment(next_start_x, next_start_y, next_end_x, next_end_y, 0xF800);

                            // 更新当前位置到A9B3
                            current_x = next_end_x;
                            current_y = next_end_y;
                            OLED_DebugMsg("Up Bypass to A9B3");

                            // 结束当前行，因为已经到达A9，且不回到原行
                            col = GRID_COLS;
                            continue;
                        }
                        else
                        {
                            // 其他情况（B1行或B7行）：直接结束当前行
                            OLED_DebugMsg("Edge Row NoFly End");
                            col = GRID_COLS;  // 结束当前行
                            continue;
                        }
                    }

                    if (row == 0)  // B1在最底部，向上绕行
                    {
                        if (row < GRID_ROWS - 1 && IsGridAccessible(row + 1, bypass_col))
                        {
                            // 正确的绕行逻辑：
                            // 1. 垂直向上到绕行行（在当前位置）
                            uint16_t detour_x = current_x;
                            uint16_t detour_y = grid_map[row + 1][col].center_y;
                            AddPathSegment(current_x, current_y, detour_x, detour_y, 0xF800);

                            // 2. 水平移动跳过禁飞区（在绕行行）
                            uint16_t safe_x = grid_map[row + 1][bypass_col].center_x;
                            uint16_t safe_y = detour_y;
                            AddPathSegment(detour_x, detour_y, safe_x, safe_y, 0xF800);

                            // 3. 垂直回到原行（在禁飞区后面）
                            next_x = safe_x;
                            next_y = grid_map[row][bypass_col].center_y;
                            AddPathSegment(safe_x, safe_y, next_x, next_y, 0xF800);

                            current_x = next_x;
                            current_y = next_y;
                            OLED_DebugMsg("Up Bypass Done");

                            // 跳过整个禁飞区
                            col = bypass_col - 1;  // -1是因为for循环会++
                        }
                    }
                    else  // B2-B7的普通绕行处理
                    {
                        if (row > 0 && IsGridAccessible(row - 1, bypass_col))
                        {
                            // 普通情况：三段式向下绕行
                            // 1. 垂直向下到绕行行（在当前位置）
                            uint16_t detour_x = current_x;
                            uint16_t detour_y = grid_map[row - 1][col].center_y;
                            AddPathSegment(current_x, current_y, detour_x, detour_y, 0xF800);

                            // 2. 水平移动跳过禁飞区（在绕行行）
                            uint16_t safe_x = grid_map[row - 1][bypass_col].center_x;
                            uint16_t safe_y = detour_y;
                            AddPathSegment(detour_x, detour_y, safe_x, safe_y, 0xF800);

                            // 3. 垂直回到原行（在禁飞区后面）
                            next_x = safe_x;
                            next_y = grid_map[row][bypass_col].center_y;
                            AddPathSegment(safe_x, safe_y, next_x, next_y, 0xF800);

                            current_x = next_x;
                            current_y = next_y;
                            OLED_DebugMsg("Down Bypass Done");

                            // 跳过整个禁飞区
                            col = bypass_col - 1;  // -1是因为for循环会++
                        }
                    }
                    continue;  // 跳过禁飞区格子
                }

                // 正常格子，添加路径
                next_x = grid_map[row][col].center_x;
                next_y = grid_map[row][col].center_y;
             //OLED_DebugMsg("123aasda");

                       //char uart_buffer321[] = "123aasda";
        //  HAL_UART_Transmit_DMA(&huart1, (uint8_t*)uart_buffer321, strlen(uart_buffer321));
                if (current_x != next_x || current_y != next_y)  // 避免重复添加同一点
                {
                    AddPathSegment(current_x, current_y, next_x, next_y, 0x07E0); // 绿色正常路径
                    OLED_DebugMsg("Normal:");
                    OLED_DebugCoord(grid_map[row][col].name, next_x, next_y);
                }

                current_x = next_x;
                current_y = next_y;
            }
        }
    }

    OLED_DebugMsg("Path Gen Done");
    OLED_DebugNum("Segments:", path_count);
    return path_count;
}
// uint16_t GenerateSnakeCruisePath(void)
// {
//     path_count = 0; // 重置路径计数
//     uint16_t current_x, current_y;
//     uint16_t next_x, next_y;
//     uint8_t current_row, current_col;

//     // 从起点开始（右下角A9,B1）
//     current_row = 0;             // B1行（底部）
//     current_col = GRID_COLS - 1; // A9列（最右）
//     current_x = grid_map[current_row][current_col].center_x;
//     current_y = grid_map[current_row][current_col].center_y;

//     OLED_ClearDebug(); // 清屏开始显示路径生成过程
//     OLED_DebugMsg("Gen Path Start:");
//     OLED_DebugCoord(grid_map[current_row][current_col].name, current_x, current_y);

//     // 蛇形扫描：从B1(row=0)往上到B7(row=6)，每行交替方向
//     for (uint8_t row = 0; row < GRID_ROWS; row++)
//     {
//         // 第一步优化：添加预分析机制
//         SimpleRowAnalysis_t analysis = AnalyzeRowForDisplay(row);

//         OLED_DebugMsg("Row Analysis:");
//         OLED_DebugNum("Row:", row);
//         OLED_DebugNum("Obstacles:", analysis.obstacle_count);

//         // 修复状态管理：在每行开始时正确更新current_x/current_y到该行的起点位置
//         if (row % 2 == 0) // 偶数行(B1,B3,B5,B7)：从右到左
//         {
//             // 偶数行起点：A9列（最右）
//             current_col = GRID_COLS - 1;
//             current_x = grid_map[row][current_col].center_x;
//             current_y = grid_map[row][current_col].center_y;
//         }
//         else // 奇数行：从左到右
//         {
//             // 奇数行起点：A1列（最左）
//             current_col = 0;
//             current_x = grid_map[row][current_col].center_x;
//             current_y = grid_map[row][current_col].center_y;
//         }

//         // 根据预分析结果选择处理策略
//         if (!analysis.has_obstacles)
//         {
//             // 无障碍物：简化处理，只生成起点到终点的路径段
//             if (row % 2 == 0) // 偶数行：从右到左
//             {
//                 next_x = grid_map[row][0].center_x;  // 终点A1
//                 next_y = grid_map[row][0].center_y;
//             }
//             else // 奇数行：从左到右
//             {
//                 next_x = grid_map[row][GRID_COLS - 1].center_x;  // 终点A9
//                 next_y = grid_map[row][GRID_COLS - 1].center_y;
//             }

//             // 添加简化的起点到终点路径段
//             AddPathSegment(current_x, current_y, next_x, next_y, 0x07E0); // 绿色正常路径
//             OLED_DebugMsg("Simple Row:");
//             OLED_DebugCoord("To", next_x, next_y);

//             // 更新当前位置到行终点
//             current_x = next_x;
//             current_y = next_y;
//         }
//         else
//         {
//             // 有障碍物：先进行特殊情况检查，然后使用详细扫描
//             OLED_DebugMsg("Detailed Row Scan");

//             // 第二步优化：添加A1B7终点特殊检查
//             if (row == GRID_ROWS - 1 && analysis.first_obstacle_col == 0)
//             {
//                 // B7行A1是禁飞区：这是最终终点，直接停止路径生成
//                 OLED_DebugMsg("A1B7 NoFly: Stop");

//                 // 只生成到禁飞区前的路径段（如果有可访问区域）
//                 if (analysis.last_obstacle_col < GRID_COLS - 1)
//                 {
//                     next_x = grid_map[row][analysis.last_obstacle_col + 1].center_x;
//                     next_y = grid_map[row][analysis.last_obstacle_col + 1].center_y;
//                     AddPathSegment(current_x, current_y, next_x, next_y, 0x07E0);
//                     current_x = next_x;
//                     current_y = next_y;
//                 }

//                 // 直接跳到行间转换处理，不继续当前行的扫描
//                 goto row_transition;
//             }

//             // 第二步优化：实现边缘禁飞区的两段式绕行优化
//             uint8_t use_two_segment_bypass = 0;

//             if (row % 2 == 0) // 偶数行：从右到左
//             {
//                 // 检查是否为边缘禁飞区（最左边）
//                 if (analysis.first_obstacle_col == 0)
//                 {
//                     use_two_segment_bypass = 1;
//                     OLED_DebugMsg("Edge NoFly: 2-Segment");

//                     // 两段式绕行：生成到禁飞区前的路径
//                     if (analysis.last_obstacle_col < GRID_COLS - 1)
//                     {
//                         next_x = grid_map[row][analysis.last_obstacle_col + 1].center_x;
//                         next_y = grid_map[row][analysis.last_obstacle_col + 1].center_y;
//                         AddPathSegment(current_x, current_y, next_x, next_y, 0x07E0);
//                         current_x = next_x;
//                         current_y = next_y;
//                     }

//                     // 垂直移动到绕行行
//                     uint8_t detour_row = (row == 0) ? row + 1 : row - 1;
//                     next_x = current_x;
//                     next_y = grid_map[detour_row][analysis.last_obstacle_col + 1].center_y;
//                     AddPathSegment(current_x, current_y, next_x, next_y, 0xF800);

//                     // 水平移动到A1（不回到原行）
//                     current_x = next_x;
//                     current_y = next_y;
//                     next_x = grid_map[detour_row][0].center_x;
//                     next_y = grid_map[detour_row][0].center_y;
//                     AddPathSegment(current_x, current_y, next_x, next_y, 0xF800);

//                     current_x = next_x;
//                     current_y = next_y;

//                     // 跳到行间转换处理
//                     goto row_transition;
//                 }
//             }
//             else // 奇数行：从左到右
//             {
//                 // 检查是否为边缘禁飞区（最右边）
//                 if (analysis.last_obstacle_col == GRID_COLS - 1)
//                 {
//                     use_two_segment_bypass = 1;
//                     OLED_DebugMsg("Edge NoFly: 2-Segment");

//                     // 两段式绕行：生成到禁飞区前的路径
//                     if (analysis.first_obstacle_col > 0)
//                     {
//                         next_x = grid_map[row][analysis.first_obstacle_col - 1].center_x;
//                         next_y = grid_map[row][analysis.first_obstacle_col - 1].center_y;
//                         AddPathSegment(current_x, current_y, next_x, next_y, 0x07E0);
//                         current_x = next_x;
//                         current_y = next_y;
//                     }

//                     // 垂直移动到绕行行
//                     uint8_t detour_row = (row == 0) ? row + 1 : row - 1;
//                     next_x = current_x;
//                     next_y = grid_map[detour_row][analysis.first_obstacle_col - 1].center_y;
//                     AddPathSegment(current_x, current_y, next_x, next_y, 0xF800);

//                     // 水平移动到A9（不回到原行）
//                     current_x = next_x;
//                     current_y = next_y;
//                     next_x = grid_map[detour_row][GRID_COLS - 1].center_x;
//                     next_y = grid_map[detour_row][GRID_COLS - 1].center_y;
//                     AddPathSegment(current_x, current_y, next_x, next_y, 0xF800);

//                     current_x = next_x;
//                     current_y = next_y;

//                     // 跳到行间转换处理
//                     goto row_transition;
//                 }
//             }

//             // 如果不是特殊情况，继续原有的详细扫描逻辑
//             if (row % 2 == 0) // 偶数行(B1,B3,B5,B7)：从右到左
//         {
//             for (int8_t col = GRID_COLS - 1; col >= 0; col--)
//             {
//                 if (!IsGridAccessible(row, col))
//                 {
//                     // 遇到禁飞区，垂直绕行（跳过整个禁飞区）
//                     OLED_DebugMsg("NoFly Found:");
//                     OLED_DebugMsg(grid_map[row][col].name);

//                     // 找到禁飞区的结束位置（从右到左扫描）
//                     int8_t bypass_col = col;
//                     while (bypass_col >= 0 && !IsGridAccessible(row, bypass_col))
//                     {
//                         bypass_col--; // 继续向左找到禁飞区结束
//                     }

//                     // 第二步优化：添加禁飞区延伸到边缘的特殊处理逻辑
//                     if (bypass_col < 0)
//                     {
//                         OLED_DebugMsg("NoFly to Edge: Special");

//                         // 禁飞区延伸到左边缘，从禁飞区右边开始绕行
//                         if (analysis.last_obstacle_col < GRID_COLS - 1)
//                         {
//                             // 绕行到禁飞区右边
//                             uint8_t detour_row = (row == 0) ? row + 1 : row - 1;
//                             uint8_t safe_col = analysis.last_obstacle_col + 1;

//                             // 垂直移动到绕行行
//                             uint16_t detour_x = grid_map[detour_row][safe_col].center_x;
//                             uint16_t detour_y = grid_map[detour_row][safe_col].center_y;
//                             AddPathSegment(current_x, current_y, detour_x, detour_y, 0xF800);

//                             // 垂直回到原行（在禁飞区右边）
//                             next_x = grid_map[row][safe_col].center_x;
//                             next_y = grid_map[row][safe_col].center_y;
//                             AddPathSegment(detour_x, detour_y, next_x, next_y, 0xF800);

//                             // 继续正常扫描到终点A1
//                             current_x = next_x;
//                             current_y = next_y;
//                             next_x = grid_map[row][0].center_x;
//                             next_y = grid_map[row][0].center_y;
//                             AddPathSegment(current_x, current_y, next_x, next_y, 0x07E0);

//                             current_x = next_x;
//                             current_y = next_y;
//                         }

//                         // 结束当前行处理
//                         col = -1;
//                         continue;
//                     }

//                     // 修正绕行逻辑：严格按照标准参考实现
//                     uint8_t detour_row;
//                     uint8_t use_two_segment = 0;

//                     // 绕行方向决策：与标准函数完全一致
//                     if (row == 0) // B1行特殊处理：向上绕行
//                     {
//                         detour_row = row + 1; // 向上到B2行
//                         OLED_DebugMsg("B1: Up Detour");
//                     }
//                     else // B2-B7行：向下绕行
//                     {
//                         detour_row = row - 1; // 向下绕行
//                         OLED_DebugMsg("B2-B7: Down Detour");
//                     }

//                     // 修正：按照标准函数的两段式绕行条件
//                     // 条件1：偶数行最左边禁飞区（A1位置禁飞区）
//                     if (analysis.first_obstacle_col == 0 && row != GRID_ROWS - 1) // 不是B7行
//                     {
//                         use_two_segment = 1;
//                         OLED_DebugMsg("2-Segment: Edge A1");
//                     }
//                     // 条件2：禁飞区延伸到终点A1
//                     else if (bypass_col == 0 && row != GRID_ROWS - 1)
//                     {
//                         use_two_segment = 1;
//                         OLED_DebugMsg("2-Segment: To End");
//                     }

//                     // 检查绕行行是否可访问
//                     if (detour_row < GRID_ROWS && IsGridAccessible(detour_row, bypass_col))
//                     {
//                         if (use_two_segment)
//                         {
//                             // 完全重写：严格按照标准函数的两段式绕行
//                             OLED_DebugMsg("2-Segment Bypass");

//                             // 标准函数的两段式绕行路径：
//                             // 1. 起点A9 → 禁飞区前A4 (当前行)
//                             // 2. A4向上 → A4 (绕行行)
//                             // 3. A4水平 → A1 (绕行行)

//                             uint8_t safe_col = analysis.last_obstacle_col + 1; // A4列

//                             // 1. 从当前位置到禁飞区前A4（当前行内水平移动）
//                             next_x = grid_map[row][safe_col].center_x;
//                             next_y = grid_map[row][safe_col].center_y;
//                             if (current_x != next_x || current_y != next_y)
//                             {
//                                 AddPathSegment(current_x, current_y, next_x, next_y, 0x07E0);
//                                 current_x = next_x;
//                                 current_y = next_y;
//                                 OLED_DebugMsg("To Safe Col");
//                             }

//                             // 2. 垂直向上到绕行行（A4B6 → A4B7）
//                             next_x = grid_map[detour_row][safe_col].center_x;
//                             next_y = grid_map[detour_row][safe_col].center_y;
//                             AddPathSegment(current_x, current_y, next_x, next_y, 0xF800);
//                             current_x = next_x;
//                             current_y = next_y;
//                             OLED_DebugMsg("Vertical Up");

//                             // 3. 水平到A1（A4B7 → A1B7）
//                             next_x = grid_map[detour_row][0].center_x;
//                             next_y = grid_map[detour_row][0].center_y;
//                             AddPathSegment(current_x, current_y, next_x, next_y, 0xF800);
//                             current_x = next_x;
//                             current_y = next_y;
//                             OLED_DebugMsg("Horizontal to A1");

//                             // 修正3：确保坐标同步
//                             current_col = 0; // A1列
//                             // 结束当前行处理，不回到原行
//                             col = -1;
//                         }
//                         else
//                         {
//                             // 标准三段式绕行：严格按照标准参考实现
//                             OLED_DebugMsg("3-Segment Bypass");

//                             // 1. 垂直移动到绕行行（在禁飞区后的安全位置）
//                             uint16_t detour_x = grid_map[detour_row][bypass_col].center_x;
//                             uint16_t detour_y = grid_map[detour_row][bypass_col].center_y;
//                             AddPathSegment(current_x, current_y, detour_x, detour_y, 0xF800);

//                             // 2. 水平移动跳过禁飞区（在绕行行上水平移动到禁飞区前）
//                             if (bypass_col > 0) // 确保不超出边界
//                             {
//                                 uint16_t safe_x = grid_map[detour_row][bypass_col - 1].center_x;
//                                 uint16_t safe_y = detour_y;
//                                 AddPathSegment(detour_x, detour_y, safe_x, safe_y, 0xF800);

//                                 // 3. 垂直回到原行（在禁飞区前的安全位置）
//                                 next_x = safe_x;
//                                 next_y = grid_map[row][bypass_col - 1].center_y;
//                                 AddPathSegment(safe_x, safe_y, next_x, next_y, 0xF800);

//                                 current_x = next_x;
//                                 current_y = next_y;
//                                 OLED_DebugMsg("3-Segment: Back to Row");

//                                 // 修正3：确保坐标同步
//                                 current_col = bypass_col - 1;
//                                 // 关键修正：设置到禁飞区前的位置，继续扫描剩余格子
//                                 col = bypass_col - 1;
//                             }
//                             else
//                             {
//                                 // 禁飞区延伸到A1，直接结束当前行
//                                 current_x = detour_x;
//                                 current_y = detour_y;
//                                 col = -1; // 结束当前行
//                             }
//                         }
//                     }
//                     continue; // 跳过禁飞区格子
//                 }

//                 // 正常格子，添加路径
//                 next_x = grid_map[row][col].center_x;
//                 next_y = grid_map[row][col].center_y;

//                 if (current_x != next_x || current_y != next_y) // 避免重复添加同一点
//                 {
//                     AddPathSegment(current_x, current_y, next_x, next_y, 0x07E0); // 绿色正常路径
//                     OLED_DebugMsg("Normal:");
//                     OLED_DebugCoord(grid_map[row][col].name, next_x, next_y);
//                 }

//                 current_x = next_x;
//                 current_y = next_y;
//             }
//         }
//         else // 奇数行(B2,B4,B6)：从左到右
//         {
//             for (uint8_t col = 0; col < GRID_COLS; col++)
//             {
//                 if (!IsGridAccessible(row, col))
//                 {
//                     // 遇到禁飞区，垂直绕行（跳过整个禁飞区）
//                     OLED_DebugMsg("NoFly Found:");
//                     OLED_DebugMsg(grid_map[row][col].name);

//                     // 找到禁飞区的结束位置（从左到右扫描）
//                     uint8_t bypass_col = col;
//                     while (bypass_col < GRID_COLS && !IsGridAccessible(row, bypass_col))
//                     {
//                         bypass_col++; // 继续向右找到禁飞区结束
//                     }

//                     // 第二步优化：添加禁飞区延伸到边缘的特殊处理逻辑
//                     if (bypass_col >= GRID_COLS)
//                     {
//                         OLED_DebugMsg("NoFly to Edge: Special");

//                         // 禁飞区延伸到右边缘，从禁飞区左边开始绕行
//                         if (analysis.first_obstacle_col > 0)
//                         {
//                             // 绕行到禁飞区左边
//                             uint8_t detour_row = (row == 0) ? row + 1 : row - 1;
//                             uint8_t safe_col = analysis.first_obstacle_col - 1;

//                             // 垂直移动到绕行行
//                             uint16_t detour_x = grid_map[detour_row][safe_col].center_x;
//                             uint16_t detour_y = grid_map[detour_row][safe_col].center_y;
//                             AddPathSegment(current_x, current_y, detour_x, detour_y, 0xF800);

//                             // 垂直回到原行（在禁飞区左边）
//                             next_x = grid_map[row][safe_col].center_x;
//                             next_y = grid_map[row][safe_col].center_y;
//                             AddPathSegment(detour_x, detour_y, next_x, next_y, 0xF800);

//                             // 继续正常扫描到终点A9
//                             current_x = next_x;
//                             current_y = next_y;
//                             next_x = grid_map[row][GRID_COLS - 1].center_x;
//                             next_y = grid_map[row][GRID_COLS - 1].center_y;
//                             AddPathSegment(current_x, current_y, next_x, next_y, 0x07E0);

//                             current_x = next_x;
//                             current_y = next_y;
//                         }

//                         // 结束当前行处理
//                         col = GRID_COLS;
//                         continue;
//                     }

//                     // 修正绕行逻辑：严格按照标准参考实现（奇数行）
//                     uint8_t detour_row;
//                     uint8_t use_two_segment = 0;

//                     // 绕行方向决策：与标准函数完全一致
//                     if (row == 0) // B1行特殊处理：向上绕行
//                     {
//                         detour_row = row + 1; // 向上到B2行
//                         OLED_DebugMsg("B1: Up Detour");
//                     }
//                     else // B2-B7行：向下绕行
//                     {
//                         detour_row = row - 1; // 向下绕行
//                         OLED_DebugMsg("B2-B7: Down Detour");
//                     }

//                     // 修正：按照标准函数的两段式绕行条件（奇数行）
//                     // 条件1：奇数行最右边禁飞区（A9位置禁飞区）
//                     if (analysis.last_obstacle_col == GRID_COLS - 1 && row != GRID_ROWS - 1) // 不是B7行
//                     {
//                         use_two_segment = 1;
//                         OLED_DebugMsg("2-Segment: Edge A9");
//                     }
//                     // 条件2：禁飞区延伸到终点A9
//                     else if (bypass_col >= GRID_COLS && row != GRID_ROWS - 1)
//                     {
//                         use_two_segment = 1;
//                         OLED_DebugMsg("2-Segment: To End");
//                     }

//                     // 检查绕行行是否可访问
//                     if (detour_row < GRID_ROWS && bypass_col < GRID_COLS && IsGridAccessible(detour_row, bypass_col))
//                     {
//                         if (use_two_segment)
//                         {
//                             // 完全重写：严格按照标准函数的两段式绕行（奇数行）
//                             OLED_DebugMsg("2-Segment Bypass");

//                             // 标准函数的两段式绕行路径（奇数行）：
//                             // 1. 起点A1 → 禁飞区前A6 (当前行)
//                             // 2. A6向上 → A6 (绕行行)
//                             // 3. A6水平 → A9 (绕行行)

//                             uint8_t safe_col = analysis.first_obstacle_col - 1; // 禁飞区前列

//                             // 1. 从当前位置到禁飞区前（当前行内水平移动）
//                             next_x = grid_map[row][safe_col].center_x;
//                             next_y = grid_map[row][safe_col].center_y;
//                             if (current_x != next_x || current_y != next_y)
//                             {
//                                 AddPathSegment(current_x, current_y, next_x, next_y, 0x07E0);
//                                 current_x = next_x;
//                                 current_y = next_y;
//                                 OLED_DebugMsg("To Safe Col");
//                             }

//                             // 2. 垂直向上到绕行行
//                             next_x = grid_map[detour_row][safe_col].center_x;
//                             next_y = grid_map[detour_row][safe_col].center_y;
//                             AddPathSegment(current_x, current_y, next_x, next_y, 0xF800);
//                             current_x = next_x;
//                             current_y = next_y;
//                             OLED_DebugMsg("Vertical Up");

//                             // 3. 水平到A9
//                             next_x = grid_map[detour_row][GRID_COLS - 1].center_x;
//                             next_y = grid_map[detour_row][GRID_COLS - 1].center_y;
//                             AddPathSegment(current_x, current_y, next_x, next_y, 0xF800);
//                             current_x = next_x;
//                             current_y = next_y;
//                             OLED_DebugMsg("Horizontal to A9");

//                             // 修正3：确保坐标同步
//                             current_col = GRID_COLS - 1; // A9列
//                             // 结束当前行处理，不回到原行
//                             col = GRID_COLS;
//                         }
//                         else
//                         {
//                             // 标准三段式绕行：严格按照标准参考实现（奇数行）
//                             OLED_DebugMsg("3-Segment Bypass");

//                             // 1. 垂直移动到绕行行（在禁飞区后的安全位置）
//                             uint16_t detour_x = grid_map[detour_row][bypass_col].center_x;
//                             uint16_t detour_y = grid_map[detour_row][bypass_col].center_y;
//                             AddPathSegment(current_x, current_y, detour_x, detour_y, 0xF800);

//                             // 2. 水平移动跳过禁飞区（在绕行行上水平移动到禁飞区后）
//                             if (bypass_col < GRID_COLS) // 确保不超出边界
//                             {
//                                 uint16_t safe_x = grid_map[detour_row][bypass_col].center_x;
//                                 uint16_t safe_y = detour_y;
//                                 // 如果已经在正确位置，不需要水平移动
//                                 if (detour_x != safe_x)
//                                 {
//                                     AddPathSegment(detour_x, detour_y, safe_x, safe_y, 0xF800);
//                                 }

//                                 // 3. 垂直回到原行（在禁飞区后的安全位置）
//                                 next_x = safe_x;
//                                 next_y = grid_map[row][bypass_col].center_y;
//                                 AddPathSegment(safe_x, safe_y, next_x, next_y, 0xF800);

//                                 current_x = next_x;
//                                 current_y = next_y;
//                                 OLED_DebugMsg("3-Segment: Back to Row");

//                                 // 修正3：确保坐标同步
//                                 current_col = bypass_col;
//                                 // 关键修正：设置到禁飞区后的位置，继续扫描剩余格子
//                                 // -1是因为for循环会++，实际会从bypass_col开始继续
//                                 col = bypass_col - 1;
//                             }
//                             else
//                             {
//                                 // 禁飞区延伸到A9，直接结束当前行
//                                 current_x = detour_x;
//                                 current_y = detour_y;
//                                 col = GRID_COLS; // 结束当前行
//                             }
//                         }
//                     }
//                     continue; // 跳过禁飞区格子
//                 }

//                 // 正常格子，添加路径
//                 next_x = grid_map[row][col].center_x;
//                 next_y = grid_map[row][col].center_y;

//                 if (current_x != next_x || current_y != next_y) // 避免重复添加同一点
//                 {
//                     AddPathSegment(current_x, current_y, next_x, next_y, 0x07E0); // 绿色正常路径
//                     OLED_DebugMsg("Normal:");
//                     OLED_DebugCoord(grid_map[row][col].name, next_x, next_y);
//                 }

//                 current_x = next_x;
//                 current_y = next_y;
//                 // 修正3：确保坐标同步
//                 current_col = col;
//             }
//             } // 结束详细扫描的else分支
//         } // 结束有障碍物的处理分支

//         // 行间转换处理标签（供特殊情况跳转使用）
//         row_transition:

//         // 添加行间转换路径：从当前行末尾到下一行起点的连接路径
//         if (row < GRID_ROWS - 1) // 不是最后一行
//         {
//             uint16_t next_row_start_x, next_row_start_y;

//             // 修正1：正确的蛇形转换逻辑
//             // 蛇形扫描应该是垂直转换，不是对角线转换
//             if (row % 2 == 0) // 当前行是偶数行，结束在A1
//             {
//                 // 下一行是奇数行，起点也在A1（垂直转换）
//                 next_row_start_x = grid_map[row + 1][0].center_x;
//                 next_row_start_y = grid_map[row + 1][0].center_y;
//             }
//             else // 当前行是奇数行，结束在A9
//             {
//                 // 下一行是偶数行，起点也在A9（垂直转换）
//                 next_row_start_x = grid_map[row + 1][GRID_COLS - 1].center_x;
//                 next_row_start_y = grid_map[row + 1][GRID_COLS - 1].center_y;
//             }

//             // 添加行间转换路径段
//             if (current_x != next_row_start_x || current_y != next_row_start_y)
//             {
//                 AddPathSegment(current_x, current_y, next_row_start_x, next_row_start_y, 0x001F); // 蓝色转换路径
//                 OLED_DebugMsg("Row Transition:");
//                 OLED_DebugCoord("To", next_row_start_x, next_row_start_y);

//                 // 更新当前位置到下一行起点
//                 current_x = next_row_start_x;
//                 current_y = next_row_start_y;
//             }
//         }
//     }

//     OLED_DebugMsg("Path Gen Done");
//     OLED_DebugNum("Segments:", path_count);
//     return path_count;
// }

/**
 * @brief 发送画线指令到串口屏
 * @param x1 起点X坐标
 * @param y1 起点Y坐标
 * @param x2 终点X坐标
 * @param y2 终点Y坐标
 * @param color 线条颜色
 * @note 指令格式："line %d,%d,%d,%d,%d\xFF\xFF\xFF"
 */
void SendLineCommand(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, uint16_t color)
{
    char cmd_buffer[64];
    color = 36868;
    // 构造画线指令
    sprintf(cmd_buffer, "line %d,%d,%d,%d,%d\xFF\xFF\xFF", x1, y1, x2, y2, color);
    tjc_send_string(cmd_buffer);
    // 这里应该调用你的串口发送函数
    // 例如：HAL_UART_Transmit(&huart_screen, (uint8_t*)cmd_buffer, strlen(cmd_buffer), 1000);

    // 在OLED上显示画线指令信息
    OLED_DebugMsg("Send Line Cmd");
    OLED_DebugCoord("From", x1, y1);
    OLED_DebugCoord("To", x2, y2);
}

/**
 * @brief 执行巡航路径显示
 * @note 将生成的路径发送到串口屏显示
 */
void ExecuteCruisePathDisplay(void)
{
    // OLED_ClearDebug();
    // OLED_DebugMsg("Exec Path Display");

    for (uint16_t i = 0; i < path_count; i++)
    {
        SendLineCommand(cruise_path[i].from_x, cruise_path[i].from_y,
                        cruise_path[i].to_x, cruise_path[i].to_y,
                        cruise_path[i].color);

        // // 在快速模式下，只显示关键进度
        // if (!fast_mode || (i % 10 == 0)) // 快速模式下每10个显示一次
        // {
        //     OLED_DebugNum("Progress:", i + 1);
        // }

        // // 根据模式调整延时
        // if (!fast_mode)
        // {
        //     HAL_Delay(20); // 正常模式：慢速显示
        // }
        // else
        // {
        //     HAL_Delay(2); // 快速模式：最小延时
        // }
    }

    //OLED_DebugMsg("Display Done!");
}

/**
 * @brief 在OLED上显示网格地图信息（调试用）
 */
void PrintGridMapInfo(void)
{
    OLED_ClearDebug();
    OLED_DebugMsg("=== Grid Map ===");
    OLED_DebugNum("Size:", MAP_WIDTH);
    OLED_DebugNum("Grid:", GRID_SIZE);
    OLED_DebugNum("Rows:", GRID_ROWS);
    OLED_DebugNum("Cols:", GRID_COLS);

    if (!fast_mode)
        HAL_Delay(500); // 让用户看到基本信息

    // 显示禁飞区信息
    OLED_ClearDebug();
    OLED_DebugMsg("NoFly Zones:");

    uint8_t nofly_count = 0;
    for (uint8_t row = 0; row < GRID_ROWS; row++)
    {
        for (uint8_t col = 0; col < GRID_COLS; col++)
        {
            if (grid_map[row][col].status == GRID_NO_FLY)
            {
                OLED_DebugCoord(grid_map[row][col].name,
                                grid_map[row][col].center_x,
                                grid_map[row][col].center_y);
                nofly_count++;
            }
        }
    }

    if (nofly_count == 0)
    {
        OLED_DebugMsg("No NoFly Zone");
    }

    if (!fast_mode)
        HAL_Delay(500); // 让用户看到禁飞区信息
}

/**
 * @brief 巡航系统测试函数
 * @note 完整的测试流程，演示你的思路实现
 */
void CruiseSystemTest(void)
{
    OLED_ClearDebug();
    OLED_DebugMsg("=== Cruise Test ===");
    if (!fast_mode)
        HAL_Delay(200);

    // 1. 初始化网格地图
    OLED_ClearDebug();
    OLED_DebugMsg("Step1: Init Grid");
    InitGridMap();
    if (!fast_mode)
        HAL_Delay(300);

    // 2. 设置禁飞区（示例：在B3行设置A3-A5为禁飞区）
    OLED_ClearDebug();
    OLED_DebugMsg("Step2: Set NoFly");
    SetNoFlyZone(2, 2); // B3行，从A3开始的3个格子
    if (!fast_mode)
        HAL_Delay(300);

    // 3. 打印地图信息（快速模式下跳过）
    if (!fast_mode)
    {
        OLED_ClearDebug();
        OLED_DebugMsg("Step3: Show Info");
        PrintGridMapInfo();
    }

    // 4. 生成巡航路径
    OLED_ClearDebug();
    OLED_DebugMsg("Step4: Gen Path");
    uint16_t path_segments = GenerateSnakeCruisePath();
    if (!fast_mode)
        HAL_Delay(300);

    // 5. 执行路径显示
    OLED_ClearDebug();
    OLED_DebugMsg("Step5: Display");
    ExecuteCruisePathDisplay();

    // 测试完成总结
    OLED_ClearDebug();
    OLED_DebugMsg("=== Test Done ===");
    OLED_DebugNum("Segments:", path_segments);
    OLED_DebugNum("Covered:", TOTAL_GRIDS - 3); // 减去3个禁飞区格子
}

/**
 * @brief 获取指定格子的坐标
 * @param row 行号 (0-6)
 * @param col 列号 (0-8)
 * @param x 返回X坐标
 * @param y 返回Y坐标
 * @return 1:成功, 0:失败
 */
uint8_t GetGridCoordinate(uint8_t row, uint8_t col, uint16_t *x, uint16_t *y)
{
    if (row >= GRID_ROWS || col >= GRID_COLS)
        return 0;

    *x = grid_map[row][col].center_x;
    *y = grid_map[row][col].center_y;
    return 1;
}

/**
 * @brief 清空巡航路径
 */
void ClearCruisePath(void)
{
    path_count = 0;
    OLED_DebugMsg("Path Cleared");
}

// /**
//  * @brief 纵向蛇形扫描巡航路径生成（处理纵向禁飞区）
//  * @note 按列扫描，从A9B1开始向上，遇到纵向禁飞区进行水平绕行
//  * @return 生成的路径段数量
//  */
// uint16_t GenerateVerticalSnakeCruisePath(void)
// {
//     path_count = 0; // 重置路径计数
//     uint16_t current_x, current_y;
//     uint16_t next_x, next_y;
//     uint8_t current_row, current_col;

//     // 从起点开始（右下角A9,B1）
//     current_row = 0;             // B1行（底部）
//     current_col = GRID_COLS - 1; // A9列（最右）
//     current_x = grid_map[current_row][current_col].center_x;
//     current_y = grid_map[current_row][current_col].center_y;

//     OLED_ClearDebug(); // 清屏开始显示路径生成过程
//     OLED_DebugMsg("Gen Vertical Path:");
//     OLED_DebugCoord(grid_map[current_row][current_col].name, current_x, current_y);

//     // 纵向蛇形扫描：从A9(col=8)往左到A1(col=0)，每列交替方向
//     for (int8_t col = GRID_COLS - 1; col >= 0; col--)
//     {
//         if ((GRID_COLS - 1 - col) % 2 == 0) // 偶数列(A9,A7,A5...)：从下到上
//         {
//             for (uint8_t row = 0; row < GRID_ROWS; row++)
//             {
//                 if (!IsGridAccessible(row, col))
//                 {
//                     // 遇到纵向禁飞区，水平绕行
//                     OLED_DebugMsg("Vertical NoFly:");
//                     OLED_DebugMsg(grid_map[row][col].name);

//                     // 找到纵向禁飞区的结束位置（从下到上扫描）
//                     uint8_t bypass_row = row;
//                     while (bypass_row < GRID_ROWS && !IsGridAccessible(bypass_row, col))
//                     {
//                         bypass_row++; // 继续向上找到禁飞区结束
//                     }

//                     // 如果禁飞区延伸到顶部边缘，直接结束当前列
//                     if (bypass_row >= GRID_ROWS)
//                     {
//                         OLED_DebugMsg("Top Edge Col NoFly End");

//                         // 直接结束当前列，不进行绕行
//                         row = GRID_ROWS; // 结束当前列
//                         continue;
//                     }

//                     // 水平绕行逻辑：向右绕行（如果可能）
//                     if (col < GRID_COLS - 1 && IsGridAccessible(row, col + 1))
//                     {
//                         // 1. 水平向右到绕行列（在当前位置）
//                         uint16_t detour_x = grid_map[row][col + 1].center_x;
//                         uint16_t detour_y = current_y;
//                         AddPathSegment(current_x, current_y, detour_x, detour_y, 0xF800);

//                         // 2. 垂直移动跳过禁飞区（在绕行列）
//                         uint16_t safe_x = detour_x;
//                         uint16_t safe_y = grid_map[bypass_row][col + 1].center_y;
//                         AddPathSegment(detour_x, detour_y, safe_x, safe_y, 0xF800);

//                         // 3. 水平回到原列（在禁飞区后面）
//                         next_x = grid_map[bypass_row][col].center_x;
//                         next_y = safe_y;
//                         AddPathSegment(safe_x, safe_y, next_x, next_y, 0xF800);

//                         current_x = next_x;
//                         current_y = next_y;
//                         OLED_DebugMsg("Right Bypass Done");

//                         // 跳过整个纵向禁飞区
//                         row = bypass_row - 1; // -1是因为for循环会++
//                     }
//                     else if (col > 0 && IsGridAccessible(row, col - 1)) // 向左绕行
//                     {
//                         // 1. 水平向左到绕行列（在当前位置）
//                         uint16_t detour_x = grid_map[row][col - 1].center_x;
//                         uint16_t detour_y = current_y;
//                         AddPathSegment(current_x, current_y, detour_x, detour_y, 0xF800);

//                         // 2. 垂直移动跳过禁飞区（在绕行列）
//                         uint16_t safe_x = detour_x;
//                         uint16_t safe_y = grid_map[bypass_row][col - 1].center_y;
//                         AddPathSegment(detour_x, detour_y, safe_x, safe_y, 0xF800);

//                         // 3. 水平回到原列（在禁飞区后面）
//                         next_x = grid_map[bypass_row][col].center_x;
//                         next_y = safe_y;
//                         AddPathSegment(safe_x, safe_y, next_x, next_y, 0xF800);

//                         current_x = next_x;
//                         current_y = next_y;
//                         OLED_DebugMsg("Left Bypass Done");

//                         // 跳过整个纵向禁飞区
//                         row = bypass_row - 1; // -1是因为for循环会++
//                     }
//                     else
//                     {
//                         // 左右都不能绕行的边缘情况处理（如A9列或A1列）
//                         OLED_DebugMsg("Edge Col NoFly End");

//                         // 直接结束当前列，不进行绕行
//                         row = GRID_ROWS; // 结束当前列
//                     }
//                     continue; // 跳过禁飞区格子
//                 }

//                 // 正常格子，添加路径
//                 next_x = grid_map[row][col].center_x;
//                 next_y = grid_map[row][col].center_y;

//                 if (current_x != next_x || current_y != next_y) // 避免重复添加同一点
//                 {
//                     AddPathSegment(current_x, current_y, next_x, next_y, 0x07E0); // 绿色正常路径
//                     OLED_DebugMsg("Normal:");
//                     OLED_DebugCoord(grid_map[row][col].name, next_x, next_y);
//                 }

//                 current_x = next_x;
//                 current_y = next_y;
//             }
//         }
//         else // 奇数列(A8,A6,A4...)：从上到下
//         {
//             for (int8_t row = GRID_ROWS - 1; row >= 0; row--)
//             {
//                 if (!IsGridAccessible(row, col))
//                 {
//                     // 遇到纵向禁飞区，水平绕行
//                     OLED_DebugMsg("Vertical NoFly:");
//                     OLED_DebugMsg(grid_map[row][col].name);

//                     // 找到纵向禁飞区的结束位置（从上到下扫描）
//                     int8_t bypass_row = row;
//                     while (bypass_row >= 0 && !IsGridAccessible(bypass_row, col))
//                     {
//                         bypass_row--; // 继续向下找到禁飞区结束
//                     }

//                     // 如果禁飞区延伸到底部边缘，直接结束当前列
//                     if (bypass_row < 0)
//                     {
//                         OLED_DebugMsg("Bottom Edge Col NoFly End");

//                         // 直接结束当前列，不进行绕行
//                         row = -1; // 结束当前列
//                         continue;
//                     }

//                     // 水平绕行逻辑：向右绕行（如果可能）
//                     if (col < GRID_COLS - 1 && IsGridAccessible(row, col + 1))
//                     {
//                         // 1. 水平向右到绕行列（在当前位置）
//                         uint16_t detour_x = grid_map[row][col + 1].center_x;
//                         uint16_t detour_y = current_y;
//                         AddPathSegment(current_x, current_y, detour_x, detour_y, 0xF800);

//                         // 2. 垂直移动跳过禁飞区（在绕行列）
//                         uint16_t safe_x = detour_x;
//                         uint16_t safe_y = grid_map[bypass_row][col + 1].center_y;
//                         AddPathSegment(detour_x, detour_y, safe_x, safe_y, 0xF800);

//                         // 3. 水平回到原列（在禁飞区后面）
//                         next_x = grid_map[bypass_row][col].center_x;
//                         next_y = safe_y;
//                         AddPathSegment(safe_x, safe_y, next_x, next_y, 0xF800);

//                         current_x = next_x;
//                         current_y = next_y;
//                         OLED_DebugMsg("Right Bypass Done");

//                         // 跳过整个纵向禁飞区
//                         row = bypass_row + 1; // +1是因为for循环会--
//                     }
//                     else if (col > 0 && IsGridAccessible(row, col - 1)) // 向左绕行
//                     {
//                         // 1. 水平向左到绕行列（在当前位置）
//                         uint16_t detour_x = grid_map[row][col - 1].center_x;
//                         uint16_t detour_y = current_y;
//                         AddPathSegment(current_x, current_y, detour_x, detour_y, 0xF800);

//                         // 2. 垂直移动跳过禁飞区（在绕行列）
//                         uint16_t safe_x = detour_x;
//                         uint16_t safe_y = grid_map[bypass_row][col - 1].center_y;
//                         AddPathSegment(detour_x, detour_y, safe_x, safe_y, 0xF800);

//                         // 3. 水平回到原列（在禁飞区后面）
//                         next_x = grid_map[bypass_row][col].center_x;
//                         next_y = safe_y;
//                         AddPathSegment(safe_x, safe_y, next_x, next_y, 0xF800);

//                         current_x = next_x;
//                         current_y = next_y;
//                         OLED_DebugMsg("Left Bypass Done");

//                         // 跳过整个纵向禁飞区
//                         row = bypass_row + 1; // +1是因为for循环会--
//                     }
//                     else
//                     {
//                         // 左右都不能绕行的边缘情况处理（如A9列或A1列）
//                         OLED_DebugMsg("Edge Col NoFly End");

//                         // 直接结束当前列，不进行绕行
//                         row = -1; // 结束当前列
//                     }
//                     continue; // 跳过禁飞区格子
//                 }

//                 // 正常格子，添加路径
//                 next_x = grid_map[row][col].center_x;
//                 next_y = grid_map[row][col].center_y;

//                 if (current_x != next_x || current_y != next_y) // 避免重复添加同一点
//                 {
//                     AddPathSegment(current_x, current_y, next_x, next_y, 0x07E0); // 绿色正常路径
//                     OLED_DebugMsg("Normal:");
//                     OLED_DebugCoord(grid_map[row][col].name, next_x, next_y);
//                 }

//                 current_x = next_x;
//                 current_y = next_y;
//             }
//         }
//     }

//     OLED_DebugMsg("Vertical Path Done");
//     OLED_DebugNum("Segments:", path_count);
//     return path_count;
// }

uint16_t GenerateVerticalSnakeCruisePath(void)
{
    path_count = 0;  // 重置路径计数
    uint16_t current_x, current_y;
    uint16_t next_x, next_y;
    uint8_t current_row, current_col;

    // 从起点开始（右下角A9,B1）
    current_row = 0;              // B1行（底部）
    current_col = GRID_COLS - 1;  // A9列（最右）
    current_x = grid_map[current_row][current_col].center_x;
    current_y = grid_map[current_row][current_col].center_y;

    OLED_ClearDebug();  // 清屏开始显示路径生成过程
    OLED_DebugMsg("Gen Vertical Path:");
    OLED_DebugCoord(grid_map[current_row][current_col].name, current_x, current_y);

    // 纵向蛇形扫描：从A9(col=8)往左到A1(col=0)，每列交替方向
    for (int8_t col = GRID_COLS - 1; col >= 0; col--)
    {
        // 更新当前列
        current_col = col;

        // 确定当前列的起始行
        uint8_t start_row = 0;

        // 如果当前位置已经在这一列中，从当前行开始
        if (current_col == col)
        {
            // 找到当前位置对应的行
            for (uint8_t r = 0; r < GRID_ROWS; r++)
            {
                if (grid_map[r][col].center_x == current_x && grid_map[r][col].center_y == current_y)
                {
                    start_row = r;
                    OLED_DebugMsg("Resume from current pos:");
                    OLED_DebugCoord(grid_map[r][col].name, current_x, current_y);
                    break;
                }
            }
        }

        if ((GRID_COLS - 1 - col) % 2 == 0)  // 偶数列(A9,A7,A5,A3,A1)：从下到上
        {
            for (uint8_t row = start_row; row < GRID_ROWS; row++)
            {
                if (!IsGridAccessible(row, col))
                {
                    // 遇到禁飞区，水平绕行（跳过整个禁飞区）
                    OLED_DebugMsg("NoFly Found:");
                    OLED_DebugMsg(grid_map[row][col].name);

                    // 找到禁飞区的结束位置（从下到上扫描）
                    uint8_t bypass_row = row;
                    while (bypass_row < GRID_ROWS && !IsGridAccessible(bypass_row, col))
                    {
                        bypass_row++;  // 继续向上找到禁飞区结束
                    }

                    // 修正：如果禁飞区延伸到顶边缘，需要区分处理
                    if (bypass_row >= GRID_ROWS)
                    {
                        // 特殊情况：偶数列的顶边界禁飞区，需要向左绕行
                        if (col > 0 && col < GRID_COLS - 1)  // A7, A5, A3列（排除A9和A1）
                        {
                            // 顶边界禁飞区：两段式绕行（向左绕行，不回到原列）
                            OLED_DebugMsg("Top Edge 2-Segment Bypass");

                            // 1. 水平向左到绕行列（从A5B4到A4B4，而不是从禁飞区A5B5）
                            uint16_t start_x = grid_map[row - 1][col].center_x;      // A5B4中心X（row-1是B4）
                            uint16_t start_y = grid_map[row - 1][col].center_y;      // A5B4中心Y
                            uint16_t end_x = grid_map[row - 1][col - 1].center_x;    // A4B4中心X（col-1是A4）
                            uint16_t end_y = grid_map[row - 1][col - 1].center_y;    // A4B4中心Y
                            AddPathSegment(start_x, start_y, end_x, end_y, 0xF800);

                            // 2. 垂直移动到A4B7（从A4B4到A4B7，在绕行列）
                            uint16_t next_start_x = end_x;                            // A4B4的X
                            uint16_t next_start_y = end_y;                            // A4B4的Y
                            uint16_t next_end_x = end_x;                              // 保持在A4列
                            uint16_t next_end_y = grid_map[GRID_ROWS - 1][col - 1].center_y; // A4B7的Y
                            AddPathSegment(next_start_x, next_start_y, next_end_x, next_end_y, 0xF800);

                            // 更新当前位置到A4B7
                            current_x = next_end_x;
                            current_y = next_end_y;
                            OLED_DebugMsg("Left Bypass to A4B7");

                            // 结束当前列，因为已经到达B7，且不回到原列
                            row = GRID_ROWS;
                            continue;
                        }
                        else if (col == 0)  // 只有A1列直接结束
                        {
                            // A1列：直接结束当前列
                            OLED_DebugMsg("Edge Col NoFly End");
                            row = GRID_ROWS;  // 结束当前列
                            continue;
                        }
                        // A9列（col=GRID_COLS-1）不在这里处理，继续执行后面的专门逻辑
                    }

                    // 特殊情况：偶数列底边界禁飞区（包含B1）
                    if (row == 0)  // 禁飞区包含B1（列起点）
                    {
                        // 偶数列底边界禁飞区：两段式绕行（绕行后回到原列继续）
                        OLED_DebugMsg("Even Col Bottom Edge 2-Segment Bypass");

                        uint8_t detour_col = (col > 0) ? col - 1 : col + 1; // A9列向左，其他向左，A1列向右

                        // 1. 水平到绕行列（从当前位置到绕行列的B4位置）
                        uint16_t detour_x = grid_map[bypass_row][detour_col].center_x;  // A4B4中心X
                        uint16_t detour_y = grid_map[bypass_row][detour_col].center_y;  // A4B4中心Y
                        AddPathSegment(current_x, current_y, detour_x, detour_y, 0xF800);

                        // 2. 水平回到原列的可访问起点（从A4B4到A3B4）
                        current_x = detour_x;
                        current_y = detour_y;
                        next_x = grid_map[bypass_row][col].center_x;  // A3B4中心X
                        next_y = grid_map[bypass_row][col].center_y;  // A3B4中心Y
                        AddPathSegment(current_x, current_y, next_x, next_y, 0xF800);

                        // 更新当前位置到A3B4，继续遍历A3列的剩余部分
                        current_x = next_x;
                        current_y = next_y;
                        OLED_DebugMsg("Even Col Bypass Back to A3B4");

                        // 跳过禁飞区，从B4开始继续遍历
                        row = bypass_row - 1; // -1是因为for循环会++
                        continue;
                    }

                    if (col == GRID_COLS - 1)  // A9在最右边，向左绕行
                    {
                        // 修正：A9列顶边界禁飞区使用两段式绕行
                        if (bypass_row >= GRID_ROWS)  // 顶边界禁飞区（如B5B6B7）
                        {
                            // A9列顶边界禁飞区：两段式绕行（向左绕行，不回到原列）
                            OLED_DebugMsg("A9 Top Edge 2-Segment Bypass");

                            // 1. 水平向左到绕行列（从A9B4到A8B4，而不是从禁飞区A9B5）
                            uint16_t start_x = grid_map[row - 1][col].center_x;      // A9B4中心X（row-1是B4）
                            uint16_t start_y = grid_map[row - 1][col].center_y;      // A9B4中心Y
                            uint16_t end_x = grid_map[row - 1][col - 1].center_x;    // A8B4中心X（col-1是A8）
                            uint16_t end_y = grid_map[row - 1][col - 1].center_y;    // A8B4中心Y
                            AddPathSegment(start_x, start_y, end_x, end_y, 0xF800);

                            // 2. 垂直移动到A8B7（从A8B4到A8B7，在绕行列，不回到原列）
                            uint16_t next_start_x = end_x;                            // A8B4的X
                            uint16_t next_start_y = end_y;                            // A8B4的Y
                            uint16_t next_end_x = end_x;                              // 保持在A8列
                            uint16_t next_end_y = grid_map[GRID_ROWS - 1][col - 1].center_y; // A8B7的Y
                            AddPathSegment(next_start_x, next_start_y, next_end_x, next_end_y, 0xF800);

                            // 更新当前位置到A8B7
                            current_x = next_end_x;
                            current_y = next_end_y;
                            OLED_DebugMsg("A9 Left Bypass to A8B7");

                            // 结束当前列，因为已经到达B7，且不回到原列
                            row = GRID_ROWS;
                        }
                        else if (col > 0 && IsGridAccessible(bypass_row, col - 1))
                        {
                            // 普通情况：三段式向左绕行
                            // 1. 水平向左到绕行列（在当前位置）
                            uint16_t detour_x = grid_map[row][col - 1].center_x;
                            uint16_t detour_y = current_y;
                            AddPathSegment(current_x, current_y, detour_x, detour_y, 0xF800);

                            // 2. 垂直移动跳过禁飞区（在绕行列）
                            uint16_t safe_x = detour_x;
                            uint16_t safe_y = grid_map[bypass_row][col - 1].center_y;
                            AddPathSegment(detour_x, detour_y, safe_x, safe_y, 0xF800);

                            // 3. 水平回到原列（在禁飞区后面）
                            next_x = grid_map[bypass_row][col].center_x;
                            next_y = safe_y;
                            AddPathSegment(safe_x, safe_y, next_x, next_y, 0xF800);

                            current_x = next_x;
                            current_y = next_y;
                            OLED_DebugMsg("Left Bypass Done");

                            // 跳过整个禁飞区
                            row = bypass_row;
                        }
                    }
                    else  // A8-A1都向右绕行
                    {
                        if (col < GRID_COLS - 1 && IsGridAccessible(bypass_row, col + 1))
                        {
                            // 正确的绕行逻辑：
                            // 1. 水平向右到绕行列（在当前位置）
                            uint16_t detour_x = grid_map[row][col + 1].center_x;
                            uint16_t detour_y = current_y;
                            AddPathSegment(current_x, current_y, detour_x, detour_y, 0xF800);

                            // 2. 垂直移动跳过禁飞区（在绕行列）
                            uint16_t safe_x = detour_x;
                            uint16_t safe_y = grid_map[bypass_row][col + 1].center_y;
                            AddPathSegment(detour_x, detour_y, safe_x, safe_y, 0xF800);

                            // 3. 水平回到原列（在禁飞区后面）
                            next_x = grid_map[bypass_row][col].center_x;
                            next_y = safe_y;
                            AddPathSegment(safe_x, safe_y, next_x, next_y, 0xF800);

                            current_x = next_x;
                            current_y = next_y;
                            OLED_DebugMsg("Right Bypass Done");

                            // 跳过整个禁飞区
                            row = bypass_row;
                        }
                    }
                    continue;  // 跳过禁飞区格子
                }

                // 正常格子，添加路径
                next_x = grid_map[row][col].center_x;
                next_y = grid_map[row][col].center_y;

                if (current_x != next_x || current_y != next_y)  // 避免重复添加同一点
                {
                    AddPathSegment(current_x, current_y, next_x, next_y, 0x07E0); // 绿色正常路径
                    OLED_DebugMsg("Normal:");
                    OLED_DebugCoord(grid_map[row][col].name, next_x, next_y);
                }

                current_x = next_x;
                current_y = next_y;
            }
        }
        else  // 奇数列(A8,A6,A4,A2)：从上到下
        {
            // 确定奇数列的起始行
            int8_t start_row = GRID_ROWS - 1;

            // 如果当前位置已经在这一列中，从当前行开始
            if (current_col == col)
            {
                // 找到当前位置对应的行
                for (int8_t r = GRID_ROWS - 1; r >= 0; r--)
                {
                    if (grid_map[r][col].center_x == current_x && grid_map[r][col].center_y == current_y)
                    {
                        start_row = r;
                        OLED_DebugMsg("Resume from current pos:");
                        OLED_DebugCoord(grid_map[r][col].name, current_x, current_y);
                        break;
                    }
                }
            }

            for (int8_t row = start_row; row >= 0; row--)
            {
                if (!IsGridAccessible(row, col))
                {
                    // 遇到禁飞区，水平绕行（跳过整个禁飞区）
                    OLED_DebugMsg("NoFly Found:");
                    OLED_DebugMsg(grid_map[row][col].name);

                    // 找到禁飞区的结束位置（从上到下扫描）
                    int8_t bypass_row = row;
                    while (bypass_row >= 0 && !IsGridAccessible(bypass_row, col))
                    {
                        bypass_row--;  // 继续向下找到禁飞区结束
                    }

                    // 修正：如果禁飞区延伸到底边缘，需要区分处理
                    if (bypass_row < 0)
                    {
                        // 特殊情况：奇数列的底边界禁飞区，需要向左绕行
                        if (col > 0 && col < GRID_COLS - 1)  // A6, A4, A2列（排除A8和A0）
                        {
                            // 底边界禁飞区：两段式绕行（向左绕行，不回到原列）
                            OLED_DebugMsg("Bottom Edge 2-Segment Bypass");

                            // 1. 水平向左到绕行列（从A6B4到A5B4，而不是从禁飞区A6B3）
                            uint16_t start_x = grid_map[row + 1][col].center_x;      // A6B4中心X（row+1是B4）
                            uint16_t start_y = grid_map[row + 1][col].center_y;      // A6B4中心Y
                            uint16_t end_x = grid_map[row + 1][col - 1].center_x;    // A5B4中心X（col-1是A5）
                            uint16_t end_y = grid_map[row + 1][col - 1].center_y;    // A5B4中心Y
                            AddPathSegment(start_x, start_y, end_x, end_y, 0xF800);

                            // 2. 垂直移动到A5B1（从A5B4到A5B1，在绕行列）
                            uint16_t next_start_x = end_x;                            // A5B4的X
                            uint16_t next_start_y = end_y;                            // A5B4的Y
                            uint16_t next_end_x = end_x;                              // 保持在A5列
                            uint16_t next_end_y = grid_map[0][col - 1].center_y;     // A5B1的Y
                            AddPathSegment(next_start_x, next_start_y, next_end_x, next_end_y, 0xF800);

                            // 更新当前位置到A5B1
                            current_x = next_end_x;
                            current_y = next_end_y;
                            OLED_DebugMsg("Left Bypass to A5B1");

                            // 结束当前列，因为已经到达B1，且不回到原列
                            row = -1;
                            continue;
                        }
                        else if (col == GRID_COLS - 1)  // 只有A8列直接结束
                        {
                            // A8列：直接结束当前列
                            OLED_DebugMsg("Edge Col NoFly End");
                            row = -1;  // 结束当前列
                            continue;
                        }
                        // A0列（col=0）不在这里处理，继续执行后面的专门逻辑
                    }

                    if (col == 0)  // A1在最左边，向右绕行
                    {
                        // 修正：A1列底边界禁飞区使用两段式绕行
                        if (bypass_row < 0)  // 底边界禁飞区（如B1B2B3）
                        {
                            // A1列底边界禁飞区：两段式绕行（向右绕行，不回到原列）
                            OLED_DebugMsg("A1 Bottom Edge 2-Segment Bypass");

                            // 1. 水平向右到绕行列（从A1B4到A2B4，而不是从禁飞区A1B3）
                            uint16_t start_x = grid_map[row + 1][col].center_x;      // A1B4中心X（row+1是B4）
                            uint16_t start_y = grid_map[row + 1][col].center_y;      // A1B4中心Y
                            uint16_t end_x = grid_map[row + 1][col + 1].center_x;    // A2B4中心X（col+1是A2）
                            uint16_t end_y = grid_map[row + 1][col + 1].center_y;    // A2B4中心Y
                            AddPathSegment(start_x, start_y, end_x, end_y, 0xF800);

                            // 2. 垂直移动到A2B1（从A2B4到A2B1，在绕行列，不回到原列）
                            uint16_t next_start_x = end_x;                            // A2B4的X
                            uint16_t next_start_y = end_y;                            // A2B4的Y
                            uint16_t next_end_x = end_x;                              // 保持在A2列
                            uint16_t next_end_y = grid_map[0][col + 1].center_y;     // A2B1的Y
                            AddPathSegment(next_start_x, next_start_y, next_end_x, next_end_y, 0xF800);

                            // 更新当前位置到A2B1
                            current_x = next_end_x;
                            current_y = next_end_y;
                            OLED_DebugMsg("A1 Right Bypass to A2B1");

                            // 结束当前列，因为已经到达B1，且不回到原列
                            row = -1;
                        }
                        else if (col < GRID_COLS - 1 && IsGridAccessible(bypass_row, col + 1))
                        {
                            // 普通情况：三段式向右绕行
                            // 1. 水平向右到绕行列（在当前位置）
                            uint16_t detour_x = grid_map[row][col + 1].center_x;
                            uint16_t detour_y = current_y;
                            AddPathSegment(current_x, current_y, detour_x, detour_y, 0xF800);

                            // 2. 垂直移动跳过禁飞区（在绕行列）
                            uint16_t safe_x = detour_x;
                            uint16_t safe_y = grid_map[bypass_row][col + 1].center_y;
                            AddPathSegment(detour_x, detour_y, safe_x, safe_y, 0xF800);

                            // 3. 水平回到原列（在禁飞区后面）
                            next_x = grid_map[bypass_row][col].center_x;
                            next_y = safe_y;
                            AddPathSegment(safe_x, safe_y, next_x, next_y, 0xF800);

                            current_x = next_x;
                            current_y = next_y;
                            OLED_DebugMsg("Right Bypass Done");

                            // 跳过整个禁飞区
                            row = bypass_row;
                        }
                    }
                    else  // A8-A3向左绕行，A2向右绕行
                    {
                        if (col == 1)  // A2列：向右绕行
                        {
                            if (col < GRID_COLS - 1 && IsGridAccessible(bypass_row, col + 1))
                            {
                                // A2列向右绕行逻辑：
                                // 1. 水平向右到绕行列（在当前位置）
                                uint16_t detour_x = grid_map[row][col + 1].center_x;
                                uint16_t detour_y = current_y;
                                AddPathSegment(current_x, current_y, detour_x, detour_y, 0xF800);

                                // 2. 垂直移动跳过禁飞区（在绕行列）
                                uint16_t safe_x = detour_x;
                                uint16_t safe_y = grid_map[bypass_row][col + 1].center_y;
                                AddPathSegment(detour_x, detour_y, safe_x, safe_y, 0xF800);

                                // 3. 水平回到原列（在禁飞区后面）
                                next_x = grid_map[bypass_row][col].center_x;
                                next_y = safe_y;
                                AddPathSegment(safe_x, safe_y, next_x, next_y, 0xF800);

                                current_x = next_x;
                                current_y = next_y;
                                OLED_DebugMsg("A2 Right Bypass Done");

                                // 跳过整个禁飞区
                                row = bypass_row;
                            }
                        }
                        else if (col > 1 && IsGridAccessible(bypass_row, col - 1))  // A8-A3向左绕行
                        {
                            // 正确的绕行逻辑：
                            // 1. 水平向左到绕行列（在当前位置）
                            uint16_t detour_x = grid_map[row][col - 1].center_x;
                            uint16_t detour_y = current_y;
                            AddPathSegment(current_x, current_y, detour_x, detour_y, 0xF800);

                            // 2. 垂直移动跳过禁飞区（在绕行列）
                            uint16_t safe_x = detour_x;
                            uint16_t safe_y = grid_map[bypass_row][col - 1].center_y;
                            AddPathSegment(detour_x, detour_y, safe_x, safe_y, 0xF800);

                            // 3. 水平回到原列（在禁飞区后面）
                            next_x = grid_map[bypass_row][col].center_x;
                            next_y = safe_y;
                            AddPathSegment(safe_x, safe_y, next_x, next_y, 0xF800);

                            current_x = next_x;
                            current_y = next_y;
                            OLED_DebugMsg("Left Bypass Done");

                            // 跳过整个禁飞区
                            row = bypass_row;
                        }
                    }
                    continue;  // 跳过禁飞区格子
                }

                // 正常格子，添加路径
                next_x = grid_map[row][col].center_x;
                next_y = grid_map[row][col].center_y;

                if (current_x != next_x || current_y != next_y)  // 避免重复添加同一点
                {
                    AddPathSegment(current_x, current_y, next_x, next_y, 0x07E0); // 绿色正常路径
                    OLED_DebugMsg("Normal:");
                    OLED_DebugCoord(grid_map[row][col].name, next_x, next_y);
                }

                current_x = next_x;
                current_y = next_y;
            }
        }

        // 添加列间转换路径（除了最后一列A1）
        if (col > 0)
        {
            uint8_t next_col = col - 1;
            uint16_t next_col_start_x, next_col_start_y;
            uint8_t next_col_start_row;

            // 确定下一列的起点
            if ((GRID_COLS - 1 - next_col) % 2 == 0) // 下一列是偶数列：从B1开始
            {
                next_col_start_row = 0; // B1

                // 检查偶数列的B1是否是禁飞区
                if (!IsGridAccessible(0, next_col)) // B1是禁飞区
                {
                    // 偶数列底部禁飞区：执行两段式绕行
                    OLED_DebugMsg("Even Col Bottom NoFly 2-Segment");

                    // 找到禁飞区结束位置
                    uint8_t accessible_row = 0;
                    while (accessible_row < GRID_ROWS && !IsGridAccessible(accessible_row, next_col))
                    {
                        accessible_row++;
                    }

                    if (accessible_row < GRID_ROWS)
                    {
                        // 1. 水平回到当前列的可访问点（A4B1 → A4B4）
                        uint16_t back_x = grid_map[accessible_row][col].center_x;  // A4B4
                        uint16_t back_y = grid_map[accessible_row][col].center_y;  // A4B4
                        AddPathSegment(current_x, current_y, back_x, back_y, 0xF800); // 红色绕行路径
                        OLED_DebugMsg("Back to accessible:");
                        OLED_DebugCoord(grid_map[accessible_row][col].name, back_x, back_y);

                        // 2. 水平到下一列的可访问起点（A4B4 → A3B4）
                        current_x = back_x;
                        current_y = back_y;
                        next_col_start_row = accessible_row;
                        next_col_start_x = grid_map[accessible_row][next_col].center_x;  // A3B4
                        next_col_start_y = grid_map[accessible_row][next_col].center_y;  // A3B4
                        AddPathSegment(current_x, current_y, next_col_start_x, next_col_start_y, 0xF800); // 红色绕行路径
                        OLED_DebugMsg("2-Segment to:");
                        OLED_DebugCoord(grid_map[accessible_row][next_col].name, next_col_start_x, next_col_start_y);
                    }
                }
                else
                {
                    // 正常情况：直接转换到B1
                    next_col_start_x = grid_map[next_col_start_row][next_col].center_x;
                    next_col_start_y = grid_map[next_col_start_row][next_col].center_y;
                    OLED_DebugMsg("Next Col Start:");
                    OLED_DebugCoord(grid_map[next_col_start_row][next_col].name, next_col_start_x, next_col_start_y);
                }
            }
            else // 下一列是奇数列：从B7开始
            {
                next_col_start_row = GRID_ROWS - 1; // B7

                // 检查奇数列的B7是否是禁飞区
                if (!IsGridAccessible(GRID_ROWS - 1, next_col)) // B7是禁飞区
                {
                    // 奇数列顶部禁飞区：执行两段式绕行
                    OLED_DebugMsg("Odd Col Top NoFly 2-Segment");

                    // 找到禁飞区结束位置
                    int8_t accessible_row = GRID_ROWS - 1;
                    while (accessible_row >= 0 && !IsGridAccessible(accessible_row, next_col))
                    {
                        accessible_row--;
                    }

                    if (accessible_row >= 0)
                    {
                        // 1. 水平回到当前列的可访问点（A5B7 → A5B4）
                        uint16_t back_x = grid_map[accessible_row][col].center_x;  // A5B4
                        uint16_t back_y = grid_map[accessible_row][col].center_y;  // A5B4
                        AddPathSegment(current_x, current_y, back_x, back_y, 0xF800); // 红色绕行路径
                        OLED_DebugMsg("Back to accessible:");
                        OLED_DebugCoord(grid_map[accessible_row][col].name, back_x, back_y);

                        // 2. 水平到下一列的可访问起点（A5B4 → A4B4）
                        current_x = back_x;
                        current_y = back_y;
                        next_col_start_row = accessible_row;
                        next_col_start_x = grid_map[accessible_row][next_col].center_x;  // A4B4
                        next_col_start_y = grid_map[accessible_row][next_col].center_y;  // A4B4
                        AddPathSegment(current_x, current_y, next_col_start_x, next_col_start_y, 0xF800); // 红色绕行路径
                        OLED_DebugMsg("2-Segment to:");
                        OLED_DebugCoord(grid_map[accessible_row][next_col].name, next_col_start_x, next_col_start_y);
                    }
                }
                else
                {
                    // 正常情况：直接转换到B7
                    next_col_start_x = grid_map[next_col_start_row][next_col].center_x;
                    next_col_start_y = grid_map[next_col_start_row][next_col].center_y;
                    OLED_DebugMsg("Next Col Start:");
                    OLED_DebugCoord(grid_map[next_col_start_row][next_col].name, next_col_start_x, next_col_start_y);
                }
            }

            // 避免重复航点：检查转换航点与下一列起点是否相同
            // 注意：在两段式绕行的情况下，current_x和current_y已经被更新，路径段已经添加
            if (current_x != next_col_start_x || current_y != next_col_start_y)
            {
                // 添加转换路径段（仅在正常转换情况下）
                AddPathSegment(current_x, current_y, next_col_start_x, next_col_start_y, 0x001F); // 蓝色转换路径
                OLED_DebugMsg("Col Transition:");
                OLED_DebugCoord("To", next_col_start_x, next_col_start_y);
            }

            // 更新当前位置到下一列起点
            current_x = next_col_start_x;
            current_y = next_col_start_y;
        }
    }

    OLED_DebugMsg("Vertical Path Done");
    OLED_DebugNum("Segments:", path_count);
    return path_count;
}

/**
 * @brief 设置纵向禁飞区（三个连续格子）
 * @param start_row 起始行号 (0-6)
 * @param col 列号 (0-8)
 */
void SetVerticalNoFlyZone(uint8_t start_row, uint8_t col)
{
    if (start_row >= GRID_ROWS - 2 || col >= GRID_COLS) // 确保有足够空间设置3个格子
    {
        OLED_DebugMsg("Vertical NoFly: Out Bound");
        return;
    }

    // 设置三个连续的纵向格子
    for (uint8_t i = 0; i < 3; i++)
    {
        if (start_row + i < GRID_ROWS)
        {
            grid_map[start_row + i][col].status = GRID_NO_FLY;
            OLED_DebugMsg("Set Vertical NoFly:");
            OLED_DebugMsg(grid_map[start_row + i][col].name);
        }
    }
}

/**
 * @brief 纵向禁飞区巡航系统测试函数
 * @note理
 */
void VerticalCruiseSystemTest(void)
{
    OLED_ClearDebug();
    OLED_DebugMsg("=== Vertical Test ===");
    if (!fast_mode)
        HAL_Delay(200);

    // 1. 初始化网格地图
    OLED_ClearDebug();
    OLED_DebugMsg("Step1: Init Grid");
    InitGridMap();
    if (!fast_mode)
        HAL_Delay(300);

    // 2. 设置纵向禁飞区（示例：在A6列设置B2-B4为禁飞区）
    OLED_ClearDebug();
    OLED_DebugMsg("Step2: Set Vertical NoFly");
    SetVerticalNoFlyZone(1, 5); // 从B2开始，A6列的3个格子
    if (!fast_mode)
        HAL_Delay(300);

    // 3. 打印地图信息（快速模式下跳过）
    if (!fast_mode)
    {
        OLED_ClearDebug();
        OLED_DebugMsg("Step3: Show Info");
        PrintGridMapInfo();
    }

    // 4. 生成纵向巡航路径
    OLED_ClearDebug();
    OLED_DebugMsg("Step4: Gen Vertical Path");
    uint16_t path_segments = GenerateVerticalSnakeCruisePath();
    if (!fast_mode)
        HAL_Delay(300);

    // 5. 执行路径显示
    OLED_ClearDebug();
    OLED_DebugMsg("Step5: Display");
    ExecuteCruisePathDisplay();

    // 测试完成总结
    OLED_ClearDebug();
    OLED_DebugMsg("=== Vertical Done ===");
    OLED_DebugNum("Segments:", path_segments);
    OLED_DebugNum("Covered:", TOTAL_GRIDS - 3); // 减去3个禁飞区格子
}

/**
 * @brief 快速巡航测试（无延时，直接画线）
 */
void FastCruiseTest(void)
{
    // 设置快速模式
    SetDisplayMode(1);

    // 执行快速测试
    OLED_ClearDebug();
    OLED_DebugMsg("=== Fast Test ===");

    // 初始化和设置（无延时）
    InitGridMap();
    SetNoFlyZone(3, 1); // B3行，从A3开始的3个格子
     SetNoFlyZone(4, 1); 
      SetNoFlyZone(5, 1); 

    // 生成路径（无延时）
    OLED_DebugMsg("Generating...");
    uint16_t path_segments = GenerateVerticalSnakeCruisePath();//ExecuteCruisePathDisplay();
//PathGenerateSnakeCruisePath();
    // 快速画线
    OLED_DebugMsg("Drawing Lines...");
    ExecuteCruisePathDisplay();

    // 显示结果
    OLED_ClearDebug();
    OLED_DebugMsg("=== Fast Done ===");
    OLED_DebugNum("Segments:", path_segments);
    OLED_DebugNum("Covered:", TOTAL_GRIDS - 3);

    // 恢复正常模式
    SetDisplayMode(0);
}

/**
 * @brief 简单坐标转换函数
 * @param lie_str 列坐标字符串，如"A3"
 * @param hang_str 行坐标字符串，如"B2"
 * @param row 输出行号(0-6)
 * @param col 输出列号(0-8)
 */
void ConvertCoord(char *lie_str, char *hang_str, uint8_t *row, uint8_t *col)
{
    // 转换列坐标 A1-A9 -> 0-8
    if (lie_str[0] == 'A' && lie_str[1] >= '1' && lie_str[1] <= '9')
    {
        *col = lie_str[1] - '1';
    }

    // 转换行坐标 B1-B7 -> 0-6
    if (hang_str[0] == 'B' && hang_str[1] >= '1' && hang_str[1] <= '7')
    {
        *row = hang_str[1] - '1';
    }
}

/**
 * @brief 快速测试纵向巡航（无延时）
 */
void QuickVerticalTest(void)
{
    // 设置快速模式
    SetDisplayMode(1);

    OLED_Clear();
    OLED_ShowString(0, 0, "Quick Vertical Test", 12, 0);

    // 初始化和设置
    InitGridMap();
    SetVerticalNoFlyZone(4, 0); // A6列，B2-B4

    // 生成和显示路径
    uint16_t segments = GenerateVerticalSnakeCruisePath();
    ExecuteCruisePathDisplay();

    OLED_ShowString(0, 7, "Quick Test Done", 12, 0);

    // 恢复正常模式
    SetDisplayMode(0);
}
